#include "CrossRoadsEvent.hpp"
#include "CrossRoadsState.hpp"
#include "FollowColorLaneState.hpp"
#include "TurnOverOnColorLaneState.hpp"
#include "Robot.hpp"
#include "Side.hpp"

FollowColorLaneState::FollowColorLaneState() : preferLeft(1) { }
FollowColorLaneState::FollowColorLaneState(bool aPreferLeft) : preferLeft(aPreferLeft) { }

void FollowColorLaneState::startState()
{
	//This assumes the robot is centered on a ColorLane when this state is started
	Robot::getInstance().getEngine().driveForward();
}

void FollowColorLaneState::handleEvent(Event& event)
{
	switch (event.getEventType())
	{
		case LANESTATUSCHANGED:
			handleLaneEvent((LaneEvent&) event);
			break;
		case CROSSROADSSTATUSCHANGED:
			if (((CrossRoadsEvent&) event).isOnCrossRoads())
				Robot::getInstance().changeState(new CrossRoadsState());
			break;
		//Obstacle detection
		case PROXIMITY:
		case COLLISION:
			//Obstacle detected! --> change state to TurnOverOnColorLaneState
			Robot::getInstance().changeState(new TurnOverOnColorLaneState());
			break;
		default:
			break;
	}
}

void FollowColorLaneState::handleLaneEvent(LaneEvent& clEvent)
{
	switch (clEvent.newColorLaneStatus)
	{
		case RIGHTON: Robot::getInstance().getEngine().turnLeft(); break;
		case LEFTON: Robot::getInstance().getEngine().turnRight(); break;
		case BOTHON: Robot::getInstance().getEngine().driveForward(); break;
		default:
			if (preferLeft) Robot::getInstance().getEngine().turnLeft();
			else Robot::getInstance().getEngine().turnRight();
		break;
	}
}

State::StateType FollowColorLaneState::getStateType()
{
	return State::FOLLOWCOLORLANE;
}
